/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef COORDINATEMANAGER_H
#define COORDINATEMANAGER_H

#include "MacroDefine/GeneralDefine.h"
#include "trajectoryudefine.h"
#include "utcalculate.h"
//#include "coordinatesolver.h"
#include <QMutex>
#include <QJsonObject>


class MotionMessage;
class CoordinateSolver;
//class LaserCameraCalibrate;
//class UtCalculate;
/**
 * @brief The CoordinateManager class　坐标系的管理
 */
class CoordinateManager
{
public://todo 可能要加锁保护。
    CoordinateManager(RobotCommonParameter * roboParameterIn, MotionMessage *motionMessageIn);

public:
    int getBestPositionFromAngle(double idealAngle,double initialPosition,double &bestPositionOut);
    int getIdealJointTorque(const QVector<double>& q_in,  const QVector<double> &tmpQdot,
                            const QVector<double> &tmpQdotdot,
                            const QVector<double> &f_tool,QVector<double> &torqueOut);
    int setGravityVector(vector<double> gravityIn);
    int setToolMass(int toolIndex, double mass, vector<double> center);
    int setSegmentAttribute(int index,JointAttribute attributeIn);
    ////工具号必须和标定时工具号一致，而且标定后，工具参数不得改变。现在记录的是相机和工具之间的偏移。
    int getWeldPositionByCameraTransfer(const PointPro tcpPosition,double x,double y,double z, PointPro &pointOut);
    int calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList,
                                  LaserCameraCalibrateResult &resultOut);
    int getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut);
    int activateCameraCoordinate(int index);
    int getCurrentCameraIndex();
    int pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                               const PointPro cameraPointIn,
                                               PointPro &worldPointOut);

    int initCoordinate();
    bool getInitialStatus();
    int writeCoordinateFile();
    int readCoordinateFile();

    /**
     * @brief getToolCoordinateInfo
     * @return
     */
    QVector<ToolCoordinateInfo> getToolCoordinateList();
    /**
     * @brief getUserCoordinateList
     * @return
     */
    QVector<UserCoordinateInfo> getUserCoordinateList();

    int getCurrentToolForwardMatrix(Frame &frameReturn);
    int getCurrentCameraForwardMatrix(Frame &frameReturn);
    int getCurrentToolInverseMatrix(Frame &frameReturn);
    int getCurrentUserForwardMatrix(Frame &frameReturn);
    int getCurrentUserInverseMatrix(Frame &frameReturn);
    int getCurrentToolIndex();

    int getCurrentUserIndex();

    int activateToolCoordinate(int index);
    int activateUserCoordinate(int index);
    int restoreUserCoordinate();
    int testCoordinate();
    int setToolCoordinate(ToolCoordinateInfo& toolCoordinate,bool isWriteFile=true);//计算工具坐标
    int setToolCoordinate_20pOfZero(ToolCoordinateInfo& toolCoordinate, vector<double> &zeroOffsetOut);
    int calculateOneAxisZeroOffset(int axisId, double deltaMove, double range, ToolCoordinateInfo& toolCoordinate, double &axisOffsetOut);
    int setUserCoordinate(UserCoordinateInfo& userCoordinate);//计算用户坐标
    int moveUserCoordinate(int coordinateIndexIn,double move_x,double move_y,double move_z);
    int copyUserCoordinate(int sourceCoordinateIndexIn,int targetCoordinateIndexIn,double offset_x,double offset_y,double offset_z);
    int equalUserCoordinate(int sourceCoordinateIndex,int targetCoordinateIndex);

//    int setJointPositon(vector<double> positionIn);

    /**
     * @brief pointCoordinateToJoint
     * @param pointIn
     * @param positionInitialList 单位 mm degree
     * @param jointPointOut
     * @return
     */
    int pointCoordinateToJoint(const PointPro pointIn,vector<double> positionInitialList,
                               PointPro &jointPointOut,bool isErrorNotice=true);
    int kinematicsInverse(std::vector<double> posIn, std::vector<double> initialJointIn
                                 , std::vector<double> &jointOut, bool isErrorNotice=true);

    /**
     * @brief pointCoordinateToCartesian, 把pointPro转为笛卡尔类型
     * @param pointIn, 输入pointPro
     * @return 输出pointPro
     */
    int pointCoordinateToCartesian(const PointPro pointIn, PointPro &pointOut);

    Frame userFrameToWorldFrame(const Frame& userFrameIn);
    Frame pointProToFrame_noUT(const PointPro& pointPro);
    int pointProToFrame_noUT(const PointPro& pointPro, Frame &frameOut);
    PointPro frameToPointPro_CART(const Frame& f);
    /**
     * @brief userFrameToPointPro_JOINT
     * @param frameIn
     * @param pointOut
     * @param configString
     * @param positionInitialList 单位　ｍｍ degree
     * @return
     */
    int userFrameToPointPro_JOINT(const Frame& frameIn, PointPro& pointOut, bool &isSinguratyOut,
                    string configString, vector<double> positionInitialList, bool isErrorNotice=true);
    int worldFrameToPointPro_JOINT(const Frame& frameIn, PointPro& pointOut, bool &isSinguratyOut,
                 string configString, vector<double> positionInitialList, bool isErrorNotice=true);
    int isPointFutCorrect(PointPro checkPoint, vector<double> initialJointPosition);
     void extractEulerMatrixAngle(const Rotation &rotMat, double &psi, double &sita, double &fai);

private:

    Rotation createEulerMatrix(double psi, double sita, double fai);
    int calculateMaxError(std::vector<PointPro> pointList,  double &maxErrorOut);
    int pointDeleteCoordinate(PointPro& decartPoint);
    int pointAddCoordinate(PointPro& decartPoint);
    int pointAddCameraCoordinate(const PointPro cameraPointIn,PointPro& decartPoint);

    int readUserCoordinateList(QString fileName);//从文件读取用户坐标系列表。
    int writeUserCoordinateList(QString fileName);//保存用户坐标系列表到文件。
    int readToolCoordinateList(QString fileName);//从文件读取工具坐标系列表。
    int writeToolCoordinateList(QString fileName);//保存工具坐标系列表到文件。
    int readCameraCoordinateList(QString fileName);//从文件读取相机坐标系列表。
    int writeCameraCoordinateList(QString fileName);//保存相机坐标系列表到文件。
    friend QDataStream& operator <<(QDataStream &out, PointPro &pointPro);
    friend QDataStream& operator >>(QDataStream &in, PointPro &pointPro);
    friend QDataStream& operator <<(QDataStream &out, Frame &frame);
    friend QDataStream& operator >>(QDataStream &in, Frame &frame);
    friend QDataStream& operator <<(QDataStream &out, ToolCoordinateInfo_frame &toolCoordinate);
    friend QDataStream& operator >>(QDataStream &in, ToolCoordinateInfo_frame &toolCoordinate);
    friend QDataStream& operator <<(QDataStream &out, UserCoordinateInfo_frame &userCoordinate);
    friend QDataStream& operator >>(QDataStream &in, UserCoordinateInfo_frame &userCoordinate);

    int readCoordinateFile_json(QString fileName,QVector<CommonCoordinateInfo_frame> &coordinateListOut);
    int writeCoordinateFile_json(QString fileNameIn,const QVector<CommonCoordinateInfo_frame> &coordinateListIn);


    int getCoordinateFromJson(const QJsonObject &obj_coordinate, CommonCoordinateInfo_frame &coordinateOut);
    int transCoordinateToJson(const CommonCoordinateInfo_frame &coordinateIn, QJsonObject &obj_out);

    int getPointProFromJson(const QJsonObject &obj_item_point, PointPro &pointOut);
    int transPointToJson(const PointPro &pointIn, QJsonObject &obj_out);

    int getFrameFromJson(const QJsonObject &obj_frame, Frame &frameOut);
    int transFrameToJson(const Frame &frameIn, QJsonObject &obj_out);

    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0,QString strms="");

private:

    QVector<ToolCoordinateInfo_frame> toolCoordinateList;//使用时没有锁保护？

    QVector<ToolCoordinateInfo_frame> cameratoolCoordinateList;

    QVector<UserCoordinateInfo_frame> userCoordinateList;

    volatile int currentToolIndex;
    volatile int currentCameraIndex;
    volatile int currentUserIndex;
    volatile int currentUserIndex_old;

    /**
     * @brief userCoordinateFile
     */
    QString userCoordinateFile;
    /**
     * @brief toolCoordinateFile
     */
    QString toolCoordinateFile;

    QString cameraCoordinateFile;

    CoordinateSolver *coordinateSolver;
//    vector<double> currentJointPosition;
    RobotCommonParameter * roboParameter;
    int robotId;

    MotionMessage *motionMessage;
    UtCalculate *utCalculate;
    bool isInitialOk;
    QMutex usingMutex;
//    LaserCameraCalibrate *laserCameraCalibrate;


};

#endif // COORDINATEMANAGER_H
